#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/kinova_driver_qt/qnode.hpp"

namespace kinova_driver_qt
{

QNode::QNode(int argc, char** argv, int kinova_controller_port) :
    init_argc(argc),
    init_argv(argv)
{
    kinovaControllerPort = kinova_controller_port;
    mutex = new QMutex();
    conditionHandler = new QWaitCondition();
    _kinova_status = new kinova_status();
    _kinova_status->isSendData = false;
}

QNode::~QNode()
{
    if(ros::isStarted())
    {
        ros::shutdown();
        ros::waitForShutdown();
    }
    wait();
}

bool QNode::init()
{
    ros::init(init_argc,init_argv,"kinova_driver");

    while ( ! ros::master::check() )
    {
        ROS_ERROR("roscore not exist, retrying ....");
    }

    ros::start();
    ros::NodeHandle n("~");
    //n.getParam("config_file_path",katana_config_file_path);
    // Add your ros communications here.

    kinova_init(n);

    start();

    return true;
}

bool QNode::kinova_init(ros::NodeHandle nh)
{
    kinova_controller    = new kinovaController(QHostAddress::LocalHost,kinovaControllerPort,
                                                mutex, conditionHandler, _kinova_status);

//    kinova_jointState_publisher = new JointState_publisher(&nh, kinova_controller,
//                                                           mutex, conditionHandler, _kinova_status);//farshid

    kinova_goto_homePos_server = nh.advertiseService("goto_home_pos",
                                                     &QNode::kinova_goto_home_pos_serviceCB, this);

    kinova_get_current_state_server = nh.advertiseService("get_current_state",
                                                          &QNode::kinova_get_current_state_serviceCB, this);

    movekinova_as  = new moveKinova_actionServer(nh, "/move_kinova",
                                                 boost::bind( &QNode::kinova_moveKinova_executeCB, this, _1), false);
    movekinova_as->start();

    moveFingers_as = new moveFingers_actionServer(nh, "/move_fingers",
                                                  boost::bind( &QNode::kinova_moveFingers_executeCB, this, _1 ), false);
    moveFingers_as->start();

    setJoints_as   = new setJoints_actionServer(nh, "/set_joints",
                                                boost::bind( &QNode::kinova_setJoints_executeCB, this, _1 ), false);

    emergency_state = nh.subscribe( "/RosAria/emergencyState", 10,
                                    ( boost::function < void(const std_msgs::Bool::ConstPtr&)>)
                                    boost::bind( &QNode::emergencyStateCB, this, _1 ) );

    setJoints_as->start();

    ROS_INFO("initialization completed");

    return true;
}

bool QNode::kinova_goto_home_pos_serviceCB( kinova_driver_qt::goto_home_pos::Request &req,
                                            kinova_driver_qt::goto_home_pos::Response &res )
{
    res.result.data = true;
    ROS_INFO("Kinova go to home position request recieved");
    gotoHomePos();
    return true;
}

bool QNode::kinova_get_current_state_serviceCB(kinova_driver_qt::get_current_state::Request &req,
                                               kinova_driver_qt::get_current_state::Response &res)
{
    ROS_INFO("get_current_state service called");
    log(Info,"get_current_state service called");

    Status status;

    this->getStatus(&status);

    ROS_INFO("Current state service finished successfully");
    log(Info,"Current state service finished successfully");

    if(!status.isActive)
    {
        ROS_INFO("There was a problem in getting status from kinova");
        log(Info,"There was a problem in getting status from kinova");
    }

    res.status = status;
    return true;
}

void QNode::run()
{
    ros::Rate loop_rate(1);
    while ( ros::ok() )
    {
        ros::spinOnce();
        loop_rate.sleep();
    }

    std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
    Q_EMIT rosShutdown();
}

void QNode::log( const LogLevel &level, const std::string &msg)
{
    logging_model.insertRows(logging_model.rowCount(),1);
    std::stringstream logging_model_msg;
    switch ( level )
    {
    case(Debug) :
    {
        ROS_DEBUG_STREAM(msg);
        logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Info) :
    {
        ROS_INFO_STREAM(msg);
        logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Warn) :
    {
        ROS_WARN_STREAM(msg);
        logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Error) :
    {
        ROS_ERROR_STREAM(msg);
        logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Fatal) :
    {
        ROS_FATAL_STREAM(msg);
        logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    }
    QVariant new_row(QString(logging_model_msg.str().c_str()));
    logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
    Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}


bool QNode::reach_cartesianPos(Cartesian cartesian)
{
    ROS_INFO("Sending command to server");
    bool result = kinova_controller->kinova_set_cpos(cartesian);
    std::cout<<"Result recevied from server:"<<result<<std::endl;
    return result;
}

bool QNode::reach_angularPos(Joints angular)
{
    return kinova_controller->kinova_set_apos(angular);
}

bool QNode::gotoHomePos()
{
    Cartesian cartesian;
    cartesian.position.x = 0.200249;
    cartesian.position.y = -0.236273;
    cartesian.position.z = 0.438483;
    cartesian.rotation.x = -1.514738;
    cartesian.rotation.y = 0.470630;
    cartesian.rotation.z = -3.185460;
    return reach_cartesianPos(cartesian);

    /*    Joints joints;
    joints.joint1.data = 288.0855907;
    joints.joint2.data = 148.867432;
    joints.joint3.data = 26.885;
    joints.joint4.data = 556.89;
    joints.joint5.data = 46.694;
    joints.joint6.data = 487.53;
    return reach_angularPos(joints);
*/

}

bool QNode::set_fingers(Fingers fingers)
{
    ROS_INFO("Sending set finger request");
    bool result = kinova_controller->kinova_set_fingers(fingers);
    return result;
}

void QNode::kinova_moveKinova_executeCB(const moveKinovaGoalConstPtr &goal)
{
    log(Info,"moveKinova request received");
    bool result = reach_cartesianPos(goal->cartesian);
    if(result)
    {
        ROS_INFO("Move Kinova Succeed");
        movekinova_as->setSucceeded();
    }
    else
    {
        ROS_INFO("Move Kinova Aborted");
        movekinova_as->setAborted();
    }
}

void QNode::kinova_moveFingers_executeCB(const moveFingersGoalConstPtr &goal)
{
    log(Info,"moveFingers request received");
    ROS_INFO("Move finger request received");
    std::cout<<goal->fingers;
    if(set_fingers(goal->fingers))
    {
        moveFingers_as->setSucceeded();
    }
    else
        moveFingers_as->setAborted();
}

void QNode::kinova_setJoints_executeCB(const setJointsGoalConstPtr &goal)
{
    log(Info,"setJoints request received");
    Joints joints;
    joints.joint1 = goal->joints.joint1;
    joints.joint2 = goal->joints.joint2;
    joints.joint3 = goal->joints.joint3;
    joints.joint4 = goal->joints.joint4;
    joints.joint5 = goal->joints.joint5;
    joints.joint6 = goal->joints.joint6;
    if(reach_angularPos(joints))
    {
        setJoints_as->setSucceeded();
    }
    else
        setJoints_as->setAborted();
}
bool QNode::getStatus(Status *status)
{
    *status = kinova_controller->kinova_get_status();
    return status->isActive;

}
void QNode::emergencyStateCB(const std_msgs::Bool::ConstPtr &msg)
{
    if(msg->data)
    {
        kinova_controller->kinova_stop();
    }
}

bool QNode::emulate_homePos()
{
    return kinova_controller->kinova_goto_homePos();
}

void QNode::ping_kinova()
{
    log(Info,kinova_controller->kinova_ping().toStdString());
}

void QNode::gotoNavigationPos()
{
    ROS_INFO("Moving to navigation posture");
    Cartesian cartesian;
    /*cartesian.position.x = 0.109098;
    cartesian.position.y = 0.264032;
    cartesian.position.z = 0.293867;
    cartesian.rotation.x = -3.109747;
    cartesian.rotation.y = 0.241498;
    cartesian.rotation.z = -2.691809;*/
cartesian.position.x = 0.138420;
        cartesian.position.y = 0.324570;
        cartesian.position.z = 0.360562;
        cartesian.rotation.x = -1.518314;
        cartesian.rotation.y = 0.471058;
        cartesian.rotation.z = -3.177259;
    reach_cartesianPos(cartesian);

}

}  // namespace kinova_driver_qt
